/*
 * @Author: LVGRAPE
 * @Date: 2023-11-09 10:18:42
 * @LastEditTime: 2023-11-15 10:48:59
 * @LastEditors: LVGRAPE
 * @Description:
 * @FilePath: \zino_fc_v0\zino\power\power.c
 * 要啥没啥，爱咋咋的
 */
#include <stdint.h>
#include <stdbool.h>
#include "zino.h"
#include "zino_pin.h"
#include "uart.h"
#include "delay.h"
#include "os_timer.h"
#include "at32f4xx.h"
#include "nvic.h"
#include "pwr.h"
#include "zino_shell.h"

#define DBG_TAG "power"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

typedef enum
{
    ZINO_PWR_DOWN = 1,
    ZINO_PWR_REBOOT,
    ZINO_5V_ACTIVE,
    ZINO_5V_OUT,

}zino_pwr_ctr_t;


static zino_io_t pwr_key_io = { GPIOA,GPIO_Pins_0 };

/**
 * @brief
 *
 * @param new_state ON, OFF
 */
void zino_hardware_pwr(pwr_state_t new_state)
{
    static zino_io_t pwr_ctr_io = { GPIOA,GPIO_Pins_1 };
    zino_pin_mode(&pwr_ctr_io, ZINO_PIN_MODE_OUTPUT);
    if (new_state)
    {
        rt_kprintf("system shutting down...");
        delay_ms(1000);
    }
#if defined(ZINO_I)
    //NOTE ZINO I 高电平开机,低电平关机
    zino_pin_write(&pwr_ctr_io, new_state);
#else
    //NOTE ZINO Premium 高电平关机,非高电平开机
    zino_pin_write(&pwr_ctr_io, !new_state);//
#endif
}
void ex_5v_ctrl(pwr_state_t new_state)
{
    static zino_io_t ex_5v_io = { GPIOB,GPIO_Pins_2 };
    if (new_state)
    {
        LOG_I("ex 5V power on...");
    }
    else
    {
        LOG_I("ex 5V power off...");
    }
    zino_pin_mode(&ex_5v_io, ZINO_PIN_MODE_OUTPUT);
    zino_pin_write(&ex_5v_io, new_state);
}

typedef enum adcChannelUsed {
    adc_vbat = 0,
    adc_v5,
    adc_m1,
    adc_m2,
    adc_m3,
    adc_m4,
    adc_core,
    adc_vref,
    adc_count,
}adcChannel_e;
struct pwr_vol {
    uint16_t vbat;
    uint16_t v5;
    uint16_t m1;
    uint16_t m2;
    uint16_t m3;
    uint16_t m4;
    uint16_t core;
    uint16_t vref;
};
#define DMA1_CH1_ADC1_BUFFER_SIZE (adc_count)


static uint16_t adc_raw_buffer[adc_count];;
void ADC1_Configuration(void)
{
    ADC_InitType ADC_InitStructure;
    DMA_InitType DMA_InitStructure;
    // NVIC_Config(ADC1_2_IRQn, 1, 1, ENABLE);
    NVIC_Config(DMA1_Channel1_IRQn, 1, 1, ENABLE);
    /* DMA1 Channel1 Configuration ----------------------------------------------*/
    DMA_Reset(DMA1_Channel1);
    DMA_DefaultInitParaConfig(&DMA_InitStructure);
    DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&ADC1->RDOR;
    // DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)ADC1->RDOR;
    DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)adc_raw_buffer;
    DMA_InitStructure.DMA_Direction = DMA_DIR_PERIPHERALSRC;
    DMA_InitStructure.DMA_BufferSize = DMA1_CH1_ADC1_BUFFER_SIZE;
    DMA_InitStructure.DMA_PeripheralInc = DMA_PERIPHERALINC_DISABLE;
    DMA_InitStructure.DMA_MemoryInc = DMA_MEMORYINC_ENABLE;
    DMA_InitStructure.DMA_PeripheralDataWidth = DMA_PERIPHERALDATAWIDTH_HALFWORD;
    DMA_InitStructure.DMA_MemoryDataWidth = DMA_MEMORYDATAWIDTH_HALFWORD;
    DMA_InitStructure.DMA_Mode = DMA_MODE_CIRCULAR;
    DMA_InitStructure.DMA_Priority = DMA_PRIORITY_HIGH;
    DMA_InitStructure.DMA_MTOM = DMA_MEMTOMEM_DISABLE;
    DMA_Init(DMA1_Channel1, &DMA_InitStructure);

    // /* Enable DMA1 channel1 */
    DMA_ChannelEnable(DMA1_Channel1, ENABLE);

    /* ADC1 Configuration ------------------------------------------------------*/
    ADC_StructInit(&ADC_InitStructure);
    ADC_InitStructure.ADC_Mode = ADC_Mode_Independent;
    ADC_InitStructure.ADC_ScanMode = ENABLE;
    ADC_InitStructure.ADC_ContinuousMode = DISABLE;
    ADC_InitStructure.ADC_ExternalTrig = ADC_ExternalTrig_None;
    ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
    ADC_InitStructure.ADC_NumOfChannel = adc_count;
    ADC_Init(ADC1, &ADC_InitStructure);

    ADC_ExternalTrigConvCtrl(ADC1, ENABLE);

    ADC_RegularChannelConfig(ADC1, ADC_Channel_4, 1, ADC_SampleTime_239_5);
    ADC_RegularChannelConfig(ADC1, ADC_Channel_5, 2, ADC_SampleTime_239_5);

    ADC_RegularChannelConfig(ADC1, ADC_Channel_10, 3, ADC_SampleTime_239_5);
    ADC_RegularChannelConfig(ADC1, ADC_Channel_11, 4, ADC_SampleTime_239_5);
    ADC_RegularChannelConfig(ADC1, ADC_Channel_12, 5, ADC_SampleTime_239_5);
    ADC_RegularChannelConfig(ADC1, ADC_Channel_13, 6, ADC_SampleTime_239_5);


    ADC_RegularChannelConfig(ADC1, ADC_Channel_TempSensor, 7, ADC_SampleTime_239_5);
    ADC_RegularChannelConfig(ADC1, ADC_Channel_Vrefint, 8, ADC_SampleTime_239_5);

    /*Enable internal temp vref convert channel*/
    ADC_TempSensorVrefintCtrl(ENABLE);

    /* Enable ADC1 DMA */
    ADC_DMACtrl(ADC1, ENABLE);

    /* Enable JEOC interrupt */
    ADC_INTConfig(ADC1, ADC_INT_EC, ENABLE);
    DMA_INTConfig(DMA1_Channel1, DMA_INT_TC, ENABLE);

    /* Enable ADC1 */
    ADC_Ctrl(ADC1, ENABLE);

    /* Enable ADC1 reset calibration register */
    ADC_RstCalibration(ADC1);
    /* Check the end of ADC1 reset calibration register */
    while (ADC_GetResetCalibrationStatus(ADC1))
        ;

    // /* Start ADC1 calibration */
    ADC_StartCalibration(ADC1);
    /* Check the end of ADC1 calibration */
    while (ADC_GetCalibrationStatus(ADC1))
        ;

    /* Start ADC1 Software Conversion */
    ADC_SoftwareStartConvCtrl(ADC1, ENABLE);

}

void DMA1_Channel1_IRQHandler(void)
{
    if (DMA_GetITStatus(DMA1_INT_TC1))
    {
        DMA_ClearITPendingBit(DMA1_INT_TC1);
        ADC_SoftwareStartConvCtrl(ADC1, DISABLE);
    }
}
#include "filter.h"
static Kalman_t vbfilter;
static Kalman_t v5filter;
static Kalman_t m1filter;
static Kalman_t m2filter;
static Kalman_t m3filter;
static Kalman_t m4filter;
static struct pwr_vol pwrVol;
uint16_t zino_get_vbat(void)
{
    return pwrVol.vbat;
}
uint16_t zino_get_v5(void)
{
    return pwrVol.v5;
}
uint16_t zino_get_m1(void)
{
    return pwrVol.m1;
}
uint16_t zino_get_m2(void)
{
    return pwrVol.m2;
}
uint16_t zino_get_m3(void)
{
    return pwrVol.m3;  
}
uint16_t zino_get_m4(void)
{
    return pwrVol.m4;
}
void pwr_vol_task(void* p)
{

#define VREFINT 1200
#define VBAT_DIV 2
#define V5_DIV 2
#define MainMotorCurrentMonitorRessitance 20 // 1/0.005R


    // pwrVol.vbat = adc_raw_buffer[adc_vbat] * VREFINT * VBAT_DIV / vref;
    // pwrVol.v5 = adc_raw_buffer[adc_v5] * VREFINT * V5_DIV / vref;
    // pwrVol.m1 = adc_raw_buffer[adc_m1] * VREFINT * MainMotorCurrentMonitorRessitance / vref;
    // pwrVol.m2 = adc_raw_buffer[adc_m2] * VREFINT * MainMotorCurrentMonitorRessitance / vref;
    // pwrVol.m3 = adc_raw_buffer[adc_m3] * VREFINT * MainMotorCurrentMonitorRessitance / vref;
    // pwrVol.m4 = adc_raw_buffer[adc_m4] * VREFINT * MainMotorCurrentMonitorRessitance / vref;

    uint16_t vref = adc_raw_buffer[adc_vref];

    pwrVol.vbat = KalmanFilter(&vbfilter, adc_raw_buffer[adc_vbat] * VREFINT * VBAT_DIV / vref);
    pwrVol.v5 = KalmanFilter(&v5filter, adc_raw_buffer[adc_v5] * VREFINT * V5_DIV / vref);
    pwrVol.m1 = KalmanFilter(&m1filter, adc_raw_buffer[adc_m1] );
    pwrVol.m2 = KalmanFilter(&m2filter, adc_raw_buffer[adc_m2] );
    pwrVol.m3 = KalmanFilter(&m3filter, adc_raw_buffer[adc_m3] );
    pwrVol.m4 = KalmanFilter(&m4filter, adc_raw_buffer[adc_m4] );

    // rt_kprintf("vbat:%d\t", pwrVol.vbat);
    // rt_kprintf("v5:%d\t", pwrVol.v5);
    // rt_kprintf("m1:%d\t", pwrVol.m1);
    // rt_kprintf("m2:%d\t", pwrVol.m2);
    // rt_kprintf("m3:%d\t", pwrVol.m3);
    // rt_kprintf("m4:%d\n", pwrVol.m4);

    // rt_kprintf("adc_raw_buffer:");
    // for(uint8_t i=0;i<adc_count;i++)
    // {
    //     rt_kprintf("%d \t",adc_raw_buffer[i]);
    // }
    // rt_kprintf("\n");
    /* Start ADC1 Software Conversion */
    ADC_SoftwareStartConvCtrl(ADC1, ENABLE);


}
int pwr_vol_task_init(void)
{
    ADC1_Configuration();
    static os_timer_t pwr_vol_timer;
    os_timer_init(&pwr_vol_timer, pwr_vol_task, 0);
    os_timer_start(&pwr_vol_timer, 10, 1);
    LOG_I("pwr_vol_task_init...");

    KalmanFilterInitParam(&vbfilter, .01, 50);
    KalmanFilterInitParam(&v5filter, .01, 100);
    KalmanFilterInitParam(&m1filter, .01, 100);
    KalmanFilterInitParam(&m2filter, .01, 100);
    KalmanFilterInitParam(&m3filter, .01, 100);
    KalmanFilterInitParam(&m4filter, .01, 100);

    return 0;
    // zino_pin_mode_plus(GPIOA,GPIO_Pins_4,GPIO_Mode_IN_ANALOG);
    // zino_pin_mode_plus(GPIOA,GPIO_Pins_5,GPIO_Mode_IN_ANALOG);
    // zino_pin_mode_plus(GPIOC,GPIO_Pins_0,GPIO_Mode_IN_ANALOG);
    // zino_pin_mode_plus(GPIOC,GPIO_Pins_1,GPIO_Mode_IN_ANALOG);
    // zino_pin_mode_plus(GPIOC,GPIO_Pins_2,GPIO_Mode_IN_ANALOG);
    // zino_pin_mode_plus(GPIOC,GPIO_Pins_3,GPIO_Mode_IN_ANALOG);
}
#include <string.h>
void zino_vol(int argc, char **argv)
{
    // rt_kprintf("zino_vol:0x%08x,0x%08x\n",argc,argv);
    // for(uint8_t i=0;i<argc;i++)
    // {
    //     rt_kprintf("argv[%d]:%s\n",i,argv[i]);
    // }
    if(!strncmp(argv[1], "vbat", 4))
    {
        rt_kprintf("Vbat:%d\n",zino_get_vbat());

    }
    else if(!strncmp(argv[1], "v5", 2))
    {
        rt_kprintf("V5:%d\n",zino_get_v5());

    }
    else{
        rt_kprintf("Usage: zino_vol vbat");
    }
}
ZINO_CMD_EXPORT(zino_vol, get zino voltage);
ZINO_APP_EXPORT(pwr_vol_task_init);
